﻿
#include <QMouseEvent>
#include <QtMath>
#include <QMap>
#include <QPainter>
#include "models/urdfparser.h"
#include "models/mesh.h"
#include "models/meshloader.h"
#include "models/robotlist.h"
#include "public/public.h"
#include "UrdfWidget.h"


UrdfWidget::UrdfWidget(QWidget *parent)
    : QOpenGLWidget(parent)
    , m_program(nullptr)
    , m_pParser(nullptr)
    , m_axisVBO(QOpenGLBuffer::VertexBuffer)
{
    qRegisterMetaType<MeshLoadData>("MeshLoadData");
}

UrdfWidget::~UrdfWidget()
{
    // 首先停止加载线程
    if (m_pMeshLoader) {
        // 断开所有信号连接
        disconnect(m_pMeshLoader, nullptr, this, nullptr);

        // 等待线程结束
        m_pMeshLoader->requestInterruption();  // 请求中断
        m_pMeshLoader->wait();                 // 等待线程结束

        delete m_pMeshLoader;
        m_pMeshLoader = nullptr;
    }

    // 然后清理OpenGL资源
    makeCurrent();

    // 清理网格据
    for (auto vbo : m_meshVBOs) {
        delete vbo;
    }
    for (auto vao : m_meshVAOs) {
        delete vao;
    }
    m_meshVBOs.clear();
    m_meshVAOs.clear();
    m_meshIndices.clear();

    // 清理着色器程序
    if(m_program)
    {
        delete m_program;
        m_program = NULL;
    }

    if(m_pParser)
    {
        delete m_pParser;
        m_pParser = NULL;
    }
    doneCurrent();
}



void UrdfWidget::initializeGL()
{
    initializeOpenGLFunctions();

    qDebug() << "OpenGL Version:" << reinterpret_cast<const char*>(glGetString(GL_VERSION));
    qDebug() << "GLSL Version:" << reinterpret_cast<const char*>(glGetString(GL_SHADING_LANGUAGE_VERSION));

    // 改回不透明的黑色背景
    glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_CULL_FACE);

    setupShaders();
    //setupCoordinateSystem();
}

void UrdfWidget::setupShaders()
{
    if (m_program) {
        delete m_program;
    }
    m_program = new QOpenGLShaderProgram(this);

    // 顶点着色器
    const char* vertexShaderSource = R"(
        #version 330 core
        layout (location = 0) in vec3 aPos;
        layout (location = 1) in vec3 aNormal;

        uniform mat4 model;
        uniform mat4 view;
        uniform mat4 projection;

        out vec3 FragPos;
        out vec3 Normal;
        out vec3 ViewPos;

        void main()
        {
            FragPos = vec3(model * vec4(aPos, 1.0));
            Normal = mat3(transpose(inverse(model))) * aNormal;
            gl_Position = projection * view * model * vec4(aPos, 1.0);
        }
    )";

    // 片段着色器 - 增强金属质感
    const char* fragmentShaderSource = R"(
        #version 330 core
        out vec4 FragColor;

        in vec3 FragPos;
        in vec3 Normal;

        uniform vec3 objectColor;
        uniform vec3 lightPos;
        uniform vec3 viewPos;
        uniform vec3 lightColor;

        void main()
        {
            // 基础金属色
            vec3 metalColor = objectColor * 0.95;  // 稍微降低基础颜色亮度

            // 环境光 - 使用较暗的环境光来增强对比度
            float ambientStrength = 0.4;
            vec3 ambient = ambientStrength * lightColor;

            // 漫反射 - 使用较小的系数来减少漫反射影响
            vec3 norm = normalize(Normal);
            vec3 lightDir = normalize(lightPos - FragPos);
            float diff = max(dot(norm, lightDir), 0.0);
            vec3 diffuse = diff * lightColor * 0.3;  // 减小漫反射强度

            // 镜面反射 - 增强镜面反射来模拟金属的光泽
            float specularStrength = 0.8;  // 增加镜面反射强度
            vec3 viewDir = normalize(viewPos - FragPos);
            vec3 reflectDir = reflect(-lightDir, norm);

            // 使用多层射来创造更真实的金属感
            float spec1 = pow(max(dot(viewDir, reflectDir), 0.0), 32);  // 主要高光
            float spec2 = pow(max(dot(viewDir, reflectDir), 0.0), 8);   // 次要高光

            vec3 specular = specularStrength * (spec1 * 0.7 + spec2 * 0.3) * lightColor;

            // Fresnel 效果 - 在边缘增加反射
            float fresnelFactor = pow(1.0 - max(dot(viewDir, norm), 0.0), 3.0);
            vec3 fresnel = fresnelFactor * lightColor * 0.3;

            // 最终颜色
            vec3 result = (ambient + diffuse) * metalColor + specular + fresnel;

            // 色调映射和伽马校正
            result = result / (result + vec3(1.0));  // 色调映射
            result = pow(result, vec3(1.0/2.2));     // 伽马校正

            FragColor = vec4(result, 1.0);
        }
    )";

    // 编译着色器
    if (!m_program->addShaderFromSourceCode(QOpenGLShader::Vertex, vertexShaderSource)) {
        qDebug() << "Vertex shader error:" << m_program->log();
        return;
    }

    if (!m_program->addShaderFromSourceCode(QOpenGLShader::Fragment, fragmentShaderSource)) {
        qDebug() << "Fragment shader error:" << m_program->log();
        return;
    }

    // 链接着色器程序
    if (!m_program->link()) {
        qDebug() << "Shader program link error:" << m_program->log();
        return;
    }

    // 绑定着色器程序
    if (!m_program->bind()) {
        qDebug() << "Could not bind shader program";
        return;
    }

    //qDebug() << "Shaders compiled and linked successfully";
}

void UrdfWidget::setupCoordinateSystem()
{
    // 创建坐标轴数据
    struct VertexData {
        QVector3D pos;
        QVector3D color;
    };

    VertexData vertices[] = {
        // X轴 - 红色
        {{0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}},
        {{1.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}},
        // Y轴 - 绿色
        {{0.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}},
        {{0.0f, 1.0f, 0.0f}, {0.0f, 1.0f, 0.0f}},
        // Z轴 - 蓝色
        {{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 1.0f}},
        {{0.0f, 0.0f, 1.0f}, {0.0f, 0.0f, 1.0f}}
    };

    m_axisVAO.create();
    m_axisVAO.bind();

    m_axisVBO.create();
    m_axisVBO.bind();
    m_axisVBO.allocate(vertices, sizeof(vertices));

    // 设置顶点属性
    // 位置属性
    glEnableVertexAttribArray(0);
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, sizeof(VertexData),
                          nullptr);
    // 颜色属性
    glEnableVertexAttribArray(1);
    glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, sizeof(VertexData),
                          reinterpret_cast<void*>(sizeof(QVector3D)));

    m_axisVAO.release();
    m_axisVBO.release();
}

void UrdfWidget::drawText(QPainter& painter, const QString& text)
{
    // 设置文字样式
    QFont font = painter.font();
    font.setPointSize(int(18*g_xScale));
    font.setFamily("微软雅黑");
    painter.setFont(font);

    // 设置文字颜色
    painter.setPen(Qt::white);

    // 在窗口中央绘制文字,启用自动换行
    painter.drawText(rect(), Qt::AlignCenter | Qt::TextWordWrap, text);
}

void UrdfWidget::drawProgressBar(QPainter& painter, int progress)
{
    // 圆环参数
    const int diameter = int(80*g_xScale);  // 圆环直径
    const int thickness = int(10*g_xScale);  // 圆环厚度

    // 计算圆环位置
    QRect rect = this->rect();
    int x = (rect.width() - diameter) / 2;
    int y = (rect.height() - diameter) / 2;
    QRect circleRect(x, y, diameter, diameter);

    // 设置画笔
    QPen pen;
    pen.setWidth(thickness);
    pen.setCapStyle(Qt::RoundCap);
    pen.setJoinStyle(Qt::RoundJoin);

    // 使用QColor的alpha通道来软化边缘
    QColor bgColor(50, 50, 50, 200);
    pen.setColor(bgColor);
    painter.setPen(pen);

    // 绘制背景圆环，稍微放大以避免边缘锯齿
    const qreal adjust = 0.5;
    painter.drawArc(circleRect.adjusted(thickness/2 + adjust,
                                        thickness/2 + adjust,
                                        -(thickness/2 + adjust),
                                        -(thickness/2 + adjust)),
                    0, 360 * 16);

    // 绘制进度圆环
    if (progress > 0) {
        // 使用更柔和的渐变色
        QConicalGradient gradient(circleRect.center(), 90);
        gradient.setColorAt(0.0, QColor(0, 120, 215, 255));
        gradient.setColorAt(0.5, QColor(0, 150, 235, 255));
        gradient.setColorAt(1.0, QColor(0, 180, 255, 255));

        pen.setBrush(gradient);
        painter.setPen(pen);
        painter.drawArc(circleRect.adjusted(thickness/2 + adjust,
                                            thickness/2 + adjust,
                                            -(thickness/2 + adjust),
                                            -(thickness/2 + adjust)),
                        90 * 16, -progress * 360 * 16 / 100);
    }

    // 绘制进度文字
    painter.setPen(Qt::white);
    QFont font = painter.font();
    font.setPixelSize(int(20*g_xScale));
    font.setFamily("微软雅黑");
    painter.setFont(font);
    painter.drawText(circleRect, Qt::AlignCenter, QString("%1%").arg(progress));

    // 在圆环下方绘制"Loading"文字
    font.setPixelSize(int(18*g_xScale));
    painter.setFont(font);
    QRect loadingTextRect = rect;
    loadingTextRect.setTop(circleRect.bottom() + int(10*g_xScale));
    painter.drawText(loadingTextRect, Qt::AlignHCenter | Qt::AlignTop, tr("Loading"));
}

void UrdfWidget::showEvent(QShowEvent *event)
{
    Q_UNUSED(event);
    if(m_pParser == NULL)
    {
        Robot* pCurRobot=RobotList::getInstance()->getCurRobot();
        m_pParser = new UrdfParser();
        connect(m_pParser,&UrdfParser::modelLoaded,this,&UrdfWidget::loadMesh,Qt::UniqueConnection);
        if(pCurRobot){
            qDebug()<<"path"<<getCurrentPath()+"urdf/"+pCurRobot->urdfName();
            m_pParser->setPackagePath(getCurrentPath()+"urdf/"+pCurRobot->urdfName());
            m_pParser->parseFile(getCurrentPath()+"urdf/"+pCurRobot->urdfName()+"/AzureLoong.urdf");
        }
        connect(RobotList::getInstance(),&RobotList::curRobotChanged,this,[=](Robot * pCurRobot, Robot * pLastRobot){
        if(pLastRobot)
            disconnect(pLastRobot,NULL, this,NULL);
        if(pCurRobot)
                connect(pCurRobot,&Robot::sportStateChanged, this, &UrdfWidget::updateData,Qt::UniqueConnection);
        });

        if(pCurRobot)
            connect(pCurRobot,&Robot::sportStateChanged, this, &UrdfWidget::updateData,Qt::UniqueConnection);
    }

}

void UrdfWidget::setBShowRobotData(bool newBShowRobotData)
{
    m_bShowRobotData = newBShowRobotData;
}

void UrdfWidget::setArc(const float *fArc)
{
    if(m_pParser)
    {
        QVector<JOINT> jointBody = getJointBody();
        for(int i = 0; i < jointBody.size(); ++i)
        {
            int nIndex = jointBody[i].nIndex;
            if(nIndex < 31)
            {
                m_pParser->updateJointPosition(jointBody[nIndex].strUrdf, fArc[nIndex]);
            }
        }
        update();
    }
}



void UrdfWidget::paintGL()
{
    // 清除颜色和深度缓冲区
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    // 如果需要显示文字或加载进度，使用QPainter
    if (m_showText || m_loading) {
        // 禁用OpenGL状态
        glDisable(GL_DEPTH_TEST);
        glDisable(GL_CULL_FACE);

        // 开始2D绘制
        QPainter painter;
        painter.begin(this);

        // 设置抗锯齿
        painter.setRenderHints(QPainter::Antialiasing |
                               QPainter::TextAntialiasing);

        // 使用半透明黑色背景
        painter.fillRect(rect(), QColor(0, 0, 0, 128));

        if (m_showText) {
            drawText(painter, m_displayText);
        } else if (m_loading) {
            drawProgressBar(painter, m_loadProgress);
        }

        painter.end();

        // 恢复OpenGL状态
        glEnable(GL_DEPTH_TEST);
        glEnable(GL_CULL_FACE);

        return;
    }

    // 绑定着色器程序
    if (!m_program->bind()) {
        return;
    }

    // 保存当前的OpenGL状态
    bool depthTest = glIsEnabled(GL_DEPTH_TEST);
    bool cullFace = glIsEnabled(GL_CULL_FACE);

    // 更新图矩阵
    updateViewMatrix();

    // 调整光照参数
    QVector3D lightPos(3.0f, 5.0f, 3.0f);  // 调整光源位置
    QVector3D lightColor(1.0f, 1.0f, 1.0f); // 保持白色光源
    QVector3D viewPos(0.0f, 0.0f, 5/*m_distance*/);

    m_program->setUniformValue("lightPos", lightPos);
    m_program->setUniformValue("lightColor", lightColor);
    m_program->setUniformValue("viewPos", viewPos);

    // 设置投影和视图矩阵
    m_program->setUniformValue("projection", m_projection);
    m_program->setUniformValue("view", m_view);

    // 绘制坐标系
    drawCoordinateSystem();

    // 绘制机器人模型
    drawRobot();

    // 解绑着色器程序
    m_program->release();

    // 禁用OpenGL状态以绘制2D内容
    glDisable(GL_DEPTH_TEST);
    glDisable(GL_CULL_FACE);

    // 恢复OpenGL状态
    if (depthTest) glEnable(GL_DEPTH_TEST);
    if (cullFace) glEnable(GL_CULL_FACE);
}

void UrdfWidget::drawCoordinateSystem()
{
    QMatrix4x4 model;
    model.setToIdentity();
    m_program->setUniformValue("model", model);

    m_axisVAO.bind();
    glLineWidth(2.0f);
    glDrawArrays(GL_LINES, 0, 6);
    m_axisVAO.release();
}

void UrdfWidget::drawRobot()
{
    if(m_pParser)
    {
        const QVector<Link>& links = m_pParser->getLinks();
        for (const Link& link : links) {
            if (!m_meshVAOs.contains(link.name)) {
                continue;
            }

            // 应用全局旋转
            QMatrix4x4 model = m_robotRotation * link.transform * link.visual_transform;
            model.translate(link.visual_origin);
            m_program->setUniformValue("model", model);

            // 设置颜色
            m_program->setUniformValue("objectColor", link.color);

            // 绘制网格
            m_meshVAOs[link.name]->bind();
            glDrawElements(GL_TRIANGLES, m_meshIndices[link.name].size(), GL_UNSIGNED_INT, nullptr);
            m_meshVAOs[link.name]->release();
        }
    }
}

void UrdfWidget::resizeGL(int w, int h)
{
    float aspect = float(w) / float(h ? h : 1);
    const float zNear = 0.1f, zFar = 100.0f, fov = 45.0f;

    m_projection.setToIdentity();
    m_projection.perspective(fov, aspect, zNear, zFar);
}

void UrdfWidget::updateViewMatrix()
{
    m_view.setToIdentity();
    m_view.translate(0.0f, 0.0f, -5/*m_distance*/);
    m_view.rotate(0/*m_xRot*/, 1.0f, 0.0f, 0.0f);
    m_view.rotate(0/*m_yRot*/, 0.0f, 1.0f, 0.0f);
}

void UrdfWidget::startLoadMeshes()
{

    // 清理旧的加载器
    if (m_pMeshLoader) {
        m_pMeshLoader->wait();
        delete m_pMeshLoader;
    }

    // 清理旧的网格数据
    cleanupMeshes();

    // 设置加载状态
    m_loading = true;
    m_loadProgress = 0;
    update();

    // 创建新的加载器
    m_pMeshLoader = new MeshLoader(m_pParser, this);
    connect(m_pMeshLoader, &MeshLoader::meshLoaded, this, &UrdfWidget::onMeshLoaded, Qt::QueuedConnection);
    connect(m_pMeshLoader, &MeshLoader::loadFinished, this, &UrdfWidget::onLoadFinished, Qt::QueuedConnection);
    connect(m_pMeshLoader, &MeshLoader::progressUpdated, this, &UrdfWidget::onProgressUpdated, Qt::QueuedConnection);

    m_pMeshLoader->start();
}

void UrdfWidget::cleanupMeshes()
{
    if (!isValid()) {
        qDebug() << "Invalid OpenGL context in cleanupMeshes";
        return;
    }

    makeCurrent();

    //qDebug() << "Cleaning up" << m_meshVBOs.size() << "meshes";

    for (auto vbo : m_meshVBOs) {
        if (vbo) {
            vbo->destroy();
            delete vbo;
        }
    }
    for (auto vao : m_meshVAOs) {
        if (vao) {
            vao->destroy();
            delete vao;
        }
    }
    m_meshVBOs.clear();
    m_meshVAOs.clear();
    m_meshIndices.clear();

    doneCurrent();
}

void UrdfWidget::onMeshLoaded(const MeshLoadData& data)
{
    // 确保在正确的上下文中执行
    if (!isValid()) {
        qDebug() << "Invalid OpenGL context in onMeshLoaded";
        return;
    }

    makeCurrent();  // 确保当前上下文是活动的

    if (data.vertices.isEmpty() || data.indices.isEmpty()) {
        qDebug() << "Empty mesh data for link:" << data.linkName;
        return;
    }

    // 创建VAO
    QOpenGLVertexArrayObject* vao = new QOpenGLVertexArrayObject(this);
    if (!vao->create()) {
        qDebug() << "Failed to create VAO for link:" << data.linkName;
        delete vao;
        doneCurrent();
        return;
    }
    vao->bind();

    // 创建VBO
    QOpenGLBuffer* vbo = new QOpenGLBuffer(QOpenGLBuffer::VertexBuffer);
    if (!vbo->create()) {
        qDebug() << "Failed to create VBO for link:" << data.linkName;
        delete vbo;
        delete vao;
        doneCurrent();
        return;
    }
    vbo->bind();
    vbo->setUsagePattern(QOpenGLBuffer::StaticDraw);
    vbo->allocate(data.vertices.constData(), data.vertices.size() * sizeof(Vertex));

    // 创建EBO
    QOpenGLBuffer* ebo = new QOpenGLBuffer(QOpenGLBuffer::IndexBuffer);
    if (!ebo->create()) {
        qDebug() << "Failed to create EBO for link:" << data.linkName;
        delete ebo;
        delete vbo;
        delete vao;
        doneCurrent();
        return;
    }
    ebo->bind();
    ebo->setUsagePattern(QOpenGLBuffer::StaticDraw);
    ebo->allocate(data.indices.constData(), data.indices.size() * sizeof(GLuint));

    // 设置顶点属性
    glEnableVertexAttribArray(0);
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, sizeof(Vertex),
                          reinterpret_cast<void*>(offsetof(Vertex, position)));

    glEnableVertexAttribArray(1);
    glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, sizeof(Vertex),
                          reinterpret_cast<void*>(offsetof(Vertex, normal)));

    // 存储到成员变量
    m_meshVBOs.insert(data.linkName, vbo);
    m_meshVAOs.insert(data.linkName, vao);
    m_meshIndices.insert(data.linkName, data.indices);

    // qDebug() << "Successfully loaded mesh for:" << data.linkName
    //          << "with" << data.vertices.size() << "vertices and"
    //          << data.indices.size() << "indices"
    //          << "Total meshes:" << m_meshVAOs.size();

    vao->release();
    vbo->release();
    ebo->release();

    doneCurrent();  // 释放上下文
    update();
}

void UrdfWidget::onLoadFinished(bool success)
{
    if (success) {
        m_loading = false;
        update();
    } else {
        m_showText = true;
        m_displayText = tr("Failed to load meshes");
        update();
    }

    // 清理加载器
    m_pMeshLoader->deleteLater();
    m_pMeshLoader = nullptr;
}

void UrdfWidget::onProgressUpdated(int progress)
{
    m_loadProgress = progress;
    update();
}

void UrdfWidget::loadMesh()
{
    if (!m_pParser->strState().isEmpty()) {
        m_showText = true;
        m_displayText = m_pParser->strState();
        update();
    } else {
        m_showText = false;
        startLoadMeshes();
    }
    m_robotRotation.setToIdentity();
    m_robotRotation.rotate(-90, 1, 0, 0);
    // m_robotRotation.rotate(M_PI, 0, 1, 0);
    m_robotRotation.rotate(-90, 0, 0, 1);
}

void UrdfWidget::updateData()
{
    if(m_bShowRobotData)
    {
        Robot * pRobot = RobotList::getInstance()->getCurRobot();
        if(pRobot)
        {
            SportState  state = pRobot->SportStateData();
            setArc(state.realArc);
        }
    }
}


void UrdfWidget::setRobotRotation(const QMatrix4x4 &rotation)
{
    m_robotRotation = rotation;
    update();
}
